#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <pcl/visualization/cloud_viewer.h>
//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::PointCloud2ConstPtr& pc_msg2){
    ROS_INFO("cb ok");
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    pcl::PointCloud<PointType>::Ptr pc_1(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg, *pc_1);

    pcl::PointCloud<PointType>::Ptr pc_2(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg2, *pc_2);

    Eigen::Quaternionf q_lidar1tolidar2(1,0,0,0);
    Eigen::Vector3f pos_lidar1tolidar2(4,2,0);
    Eigen::Matrix4f T_matrix = Eigen::Matrix4f::Identity();
    T_matrix.block<3, 3>(0, 0) = q_lidar1tolidar2.toRotationMatrix();
    T_matrix.block<3, 1>(0, 3) = pos_lidar1tolidar2;

    pcl::PointCloud<PointType>::Ptr cloud_add (new pcl::PointCloud<PointType> ());
    pcl::transformPointCloud (*pc_2, *pc_2, T_matrix);

    *cloud_add = (*pc_1) + (*pc_2);

    Eigen::Vector3f pos_car(-2,-1,0.7);
    Eigen::Matrix4f T_car = Eigen::Matrix4f::Identity();
    T_car.block<3, 3>(0, 0) = q_lidar1tolidar2.toRotationMatrix();
    T_car.block<3, 1>(0, 3) = pos_car;
    pcl::transformPointCloud (*cloud_add, *cloud_add, T_car);

    cout<<"pc1 size "<<pc_1->points.size()<<endl;
    cout<<"pc2 size "<<pc_2->points.size()<<endl;
    cout<<"pcadd size "<<cloud_add->points.size()<<endl;

    double hwl[3];
    int point_num_threhold_=30;
    hwl[0]=0.62;hwl[1]=0.8;hwl[2]=1.1;
    tf::Vector3 bbox_point_left_up(hwl[2]/2,hwl[1]/2,0.75),bbox_point_right_dowm(-hwl[2]/2,-hwl[1]/2,0.001);

    pcl::PointCloud<PointType>::Ptr filtered_body {new pcl::PointCloud<PointType>};//指针还是对象，有时候只能指针，有时候都行。报错就换。
    pcl::CropBox<PointType> box_filter;//滤波器对象
    box_filter.setMin(Eigen::Vector4f(bbox_point_right_dowm[0], bbox_point_right_dowm[1], bbox_point_right_dowm[2], 1.0));//Min和Max是指立方体的两个对角点。每个点由一个四维向量表示，通常最后一个是1.（不知道为什么要有四个，大神知道的给解答一下下）
    box_filter.setMax(Eigen::Vector4f(bbox_point_left_up[0], bbox_point_left_up[1], bbox_point_left_up[2], 1.0));
    box_filter.setNegative(false);//是保留立方体内的点而去除其他点，还是反之。false是将盒子内的点去除，默认为false
    box_filter.setInputCloud(cloud_add);//输入源
    box_filter.filter(*filtered_body);//滤它！

    string tranin_or_test="car";
//    string tranin_or_test="testing";
    string dataset_name = "0509";
    cout<<tranin_or_test<<endl;

    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".bin";
    pcd2bin(cloud_add,pc_path_name_);

   pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/"+frame_id_string_+".bin";
   pcd2bin(filtered_body,pc_path_name_);

    frame_num_++;

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0.5, 0.5, 0.5);
    viewer->addPointCloud<PointType>(filtered_body, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem(1.0);

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);



    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/lidar1/Ruby80", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc2_sub(nh, "/lidar2/Ruby80", 1);
//    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_left_sub, image_right_sub,pc_sub, 10);
//    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::PointCloud2> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, pc2_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.5));
    ROS_INFO("ok1");
    sync.registerCallback(boost::bind(&cloudCB, _1, _2));
    ROS_INFO("ok2");
    ros::spin();

    return 0;
}
